clear;

%% Test Ah times a vector (PASSED)
% Set parameters (single tx)

invP = invParams;

%Mesh
nc = 4;
invP.mesh = meshDM(10*ones(nc,1),10*ones(nc,1),10*ones(nc,1));
invP.activeCells = invP.mesh.CC(:,3)<sum(invP.mesh.hz)/2;

%Tx/Rx locations
invP.loc = [sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2];

%Time stepping
invP.disT = [3 1e-6];
   
%Primary Fields   
invP.h0 = calcH0_VMD(invP);

%Interpolation matrix
invP.Q = getInterpmat(invP);

%Sigma and tau models
invP.sigmaModel = 1e-8*ones(invP.mesh.nc,1);
invP.sigmaModel(invP.activeCells) = 1e-3;

invP.tauModel = zeros(invP.mesh.nc,1);
invP.tauModel(invP.activeCells) = 0.1;

% Make a model

k = 1/invP.disT(1,2);

m = abs(rand(invP.mesh.nc,1))/2 + 0.1;

T = sdiag(invP.mesh.Af*(1 + k*invP.tauModel.*(1 - m)));
Tt = sdiag(invP.mesh.Af*(k*invP.tauModel.*(1 - m)));

M = k*invP.mu*speye(invP.mesh.ne);
S = sdiag(invP.mesh.Af*(invP.sigmaModel.*(1 + k*invP.tauModel)));
St = sdiag(invP.mesh.Af*(k*invP.tauModel.*invP.sigmaModel));

% Build Ah

A = [M invP.mesh.CURL' zeros(invP.mesh.ne,invP.mesh.nf);
    invP.mesh.CURL zeros(invP.mesh.nf,invP.mesh.nf) -speye(invP.mesh.nf);
    zeros(invP.mesh.nf,invP.mesh.ne) S -T];

B = [-M zeros(size(invP.mesh.CURL')) zeros(invP.mesh.ne,invP.mesh.nf);
    zeros(size(invP.mesh.CURL)) zeros(invP.mesh.nf) zeros(invP.mesh.nf);
    zeros(invP.mesh.nf,invP.mesh.ne) -St Tt];
Ah = [A zeros(size(A)) zeros(size(A)); B A zeros(size(A)); zeros(size(A)) B A];

% Test Ah times a vector

k = 1/invP.disT(1,2);
for ii = 1:invP.nt
    j{ii} = rand(invP.mesh.nf,invP.ntx);
end

h = [];
for ii = 1:invP.nt
    h{ii} = rand(invP.mesh.ne,invP.ntx);
end

e = [];
for ii = 1:invP.nt
    e{ii} = rand(invP.mesh.nf,invP.ntx);
end

u = [];
for ii = 1:invP.nt
    u = [u; h{ii}; e{ii}; j{ii}];
end

Ahat_vec1 = Ah*u;
[hq,eq,jq] = calcAhVec(invP,m,h,e,j);

Ahat_vec2 = [];
for ii = 1:invP.ntx
    for jj = 1:invP.nt
        Ahat_vec2 = [Ahat_vec2; hq{jj}(:,ii); eq{jj}(:,ii); jq{jj}(:,ii)];
    end
end
    
figure(1);
subplot(2,1,1);
plot(Ahat_vec1(:),'o');
hold on;
plot(Ahat_vec2(:),'rx');
hold off;
subplot(2,1,2);
plot(abs(Ahat_vec1(:)-Ahat_vec2(:))./abs(Ahat_vec1(:)));

%% Test v = A\q (PASSED)
clear
invP = invParams;

%Mesh
nc = 6;
invP.mesh = meshDM(10*ones(nc,1),10*ones(nc,1),10*ones(nc,1));
invP.activeCells = invP.mesh.CC(:,3)<sum(invP.mesh.hz)/2;

%Tx/Rx locations
invP.loc = [sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2];

%Time stepping
invP.disT = [3 1e-6];
   
%Primary Fields   
invP.h0 = calcH0_VMD(invP);

%Interpolation matrix
invP.Q = getInterpmat(invP);

%Sigma and tau models
invP.sigmaModel = 1e-8*ones(invP.mesh.nc,1);
invP.sigmaModel(invP.activeCells) = 1e-3;

invP.tauModel = zeros(invP.mesh.nc,1);
invP.tauModel(invP.activeCells) = 0.1;
 
% Make a model

k = 1/invP.disT(1,2);

m = abs(rand(invP.mesh.nc,1))/2 + 0.1;

T = sdiag(invP.mesh.Af*(1 + k*invP.tauModel.*(1 - m)));
Tt = sdiag(invP.mesh.Af*(k*invP.tauModel.*(1 - m)));

M = k*invP.mu*speye(invP.mesh.ne);
S = sdiag(invP.mesh.Af*(invP.sigmaModel.*(1 + k*invP.tauModel)));
St = sdiag(invP.mesh.Af*(k*invP.tauModel.*invP.sigmaModel));

% Build Ah

A = [M invP.mesh.CURL' zeros(invP.mesh.ne,invP.mesh.nf);
    invP.mesh.CURL zeros(invP.mesh.nf,invP.mesh.nf) -speye(invP.mesh.nf);
    zeros(invP.mesh.nf,invP.mesh.ne) S -T];

B = [-M zeros(size(invP.mesh.CURL')) zeros(invP.mesh.ne,invP.mesh.nf);
    zeros(size(invP.mesh.CURL)) zeros(invP.mesh.nf) zeros(invP.mesh.nf);
    zeros(invP.mesh.nf,invP.mesh.ne) -St Tt];
Ah = [A zeros(size(A)) zeros(size(A)); B A zeros(size(A)); zeros(size(A)) B A];


q3 = [];
q = [];
for ii = 1:invP.nt
    q3{ii,1} = rand(invP.mesh.nf,invP.ntx);
    q = [q; zeros(invP.mesh.ne+invP.mesh.nf,invP.ntx); q3{ii}];
end

u_rec1 = Ah\q;
h_rec1 = [];
for ii = 1:invP.ntx
    tmp = reshape(u_rec1(:,ii),invP.mesh.ne+2*invP.mesh.nf,invP.nt);
    tmp2 = tmp(1:invP.mesh.ne,:)
    h_rec1(:,ii) = tmp2(:);
end
h_rec1 = h_rec1(:);

h_rec2 = calcAhInv(invP,m,q3);
h_rec2 = cell2mat(h_rec2);
h_rec2 = h_rec2(:);

figure(1);
subplot(2,1,1);
plot(h_rec1,'o');
hold on;
plot(h_rec2,'rx')
hold off
subplot(2,1,2);
plot(abs(h_rec1-h_rec2)./abs(h_rec1));

%%  Test forward modelling (PASSED)

clear
invP = invParams;

%Mesh
nc = 6;
invP.mesh = meshDM(10*ones(nc,1),10*ones(nc,1),10*ones(nc,1));
invP.activeCells = invP.mesh.CC(:,3)<sum(invP.mesh.hz)/2;

%Tx/Rx locations
invP.loc = [sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2];

%Time stepping
invP.disT = [3 1e-6];
   
%Primary Fields   
invP.h0 = calcH0_VMD(invP);

%Interpolation matrix
invP.Q = getInterpmat(invP);

%Sigma and tau models
invP.sigmaModel = 1e-8*ones(invP.mesh.nc,1);
invP.sigmaModel(invP.activeCells) = 1e-3;

invP.tauModel = zeros(invP.mesh.nc,1);
invP.tauModel(invP.activeCells) = 0.1;
 
% Make a model

k = 1/invP.disT(1,2);

m = abs(rand(invP.mesh.nc,1))/2 + 0.1;

T = sdiag(invP.mesh.Af*(1 + k*invP.tauModel.*(1 - m)));
Tt = sdiag(invP.mesh.Af*(k*invP.tauModel.*(1 - m)));

M = k*invP.mu*speye(invP.mesh.ne);
S = sdiag(invP.mesh.Af*(invP.sigmaModel.*(1 + k*invP.tauModel)));
St = sdiag(invP.mesh.Af*(k*invP.tauModel.*invP.sigmaModel));

% Build Ah

A = [M invP.mesh.CURL' zeros(invP.mesh.ne,invP.mesh.nf);
    invP.mesh.CURL zeros(invP.mesh.nf,invP.mesh.nf) -speye(invP.mesh.nf);
    zeros(invP.mesh.nf,invP.mesh.ne) S -T];

B = [-M zeros(size(invP.mesh.CURL')) zeros(invP.mesh.ne,invP.mesh.nf);
    zeros(size(invP.mesh.CURL)) zeros(invP.mesh.nf) zeros(invP.mesh.nf);
    zeros(invP.mesh.nf,invP.mesh.ne) -St Tt];
Ah = [A zeros(size(A)) zeros(size(A)); B A zeros(size(A)); zeros(size(A)) B A];

%set up u0
u0 = [invP.h0; zeros(2*invP.mesh.nf,invP.ntx)];
q = [-B*u0; zeros((invP.nt-1)*(invP.mesh.ne+2*invP.mesh.nf),invP.ntx)];

%Solve matrix system

u1 = Ah\q;
u1 = u1(:);

[hu2,eu2,ju2] = calcHs_TDIP(invP,m,false);

u2 = [];
for jj = 1:invP.ntx
    for ii = 1:invP.nt
        u2 = [u2; hu2{ii}(:,jj)];
        u2 = [u2; eu2{ii}(:,jj)];
        u2 = [u2; ju2{ii}(:,jj)];
    end
end

figure(1);
subplot(2,1,1);
plot(u1,'o');
hold on;
plot(u2,'rx');
hold off;
subplot(2,1,2);
plot(abs(u1-u2)./abs(u1+1e-8));

%% Derivative test for J (function) (PASSED)


clear
invP = invParams;

%Mesh
nc = 12;
invP.mesh = meshDM(10*ones(nc,1),10*ones(nc,1),10*ones(nc,1));
invP.activeCells = invP.mesh.CC(:,3)<sum(invP.mesh.hz)/2;

%Tx/Rx locations
invP.loc = [sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2];
        
%Time stepping
invP.disT = [5 1e-6];
   
%Primary Fields   
invP.h0 = calcH0_VMD(invP);

%Interpolation matrix
invP.Q = getInterpmat(invP);

%Sigma and tau models
invP.sigmaModel = 1e-8*ones(invP.mesh.nc,1);
invP.sigmaModel(invP.activeCells) = 1e-3;

invP.tauModel = zeros(invP.mesh.nc,1);
invP.tauModel(invP.activeCells) = 0.1;
 
% Make a model

k = 1/invP.disT(1,2);

m = rand(invP.mesh.nc,1);
dm = rand(invP.mesh.nc,1);

h = 0.000001;

[hm,~,jm] = calcHs_TDIP(invP,m,false);
[hmdm,~,~] = calcHs_TDIP(invP,m + h*dm,false);

dat_m = projHs(invP,hm);
dat_mdm = projHs(invP,hmdm);
J_m = Jvec_TDIP(invP,m,jm,dm);

norm(dat_mdm - dat_m)
norm(dat_mdm - dat_m - h*J_m)

%% Adjoint test for Gvec and GTvec codes (PASSED)

clear
invP = invParams;

%Mesh
nc = 12;
invP.mesh = meshDM(10*ones(nc,1),10*ones(nc,1),10*ones(nc,1));
invP.activeCells = invP.mesh.CC(:,3)<sum(invP.mesh.hz)/2;

%Tx/Rx locations
invP.loc = [sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2];
        
%Time stepping
invP.disT = [5 1e-6];
   
%Primary Fields   
invP.h0 = calcH0_VMD(invP);

%Interpolation matrix
invP.Q = getInterpmat(invP);

%Sigma and tau models
invP.sigmaModel = 1e-8*ones(invP.mesh.nc,1);
invP.sigmaModel(invP.activeCells) = 1e-3;

invP.tauModel = zeros(invP.mesh.nc,1);
invP.tauModel(invP.activeCells) = 0.1;


%Random currents
j = [];
for ii = 1:invP.nt
    j{ii} = rand(invP.mesh.nf,invP.ntx);
end

%random model
w = rand(invP.mesh.nc,1);
v = [];
for ii = 1:invP.nt
    v{ii} = rand(invP.mesh.nf,invP.ntx);
end
vv = cell2mat(v);
vv = vv(:);

t1 = Geta_vec(invP,j,w);
t1 = cell2mat(t1);
t1 = t1(:);
t1 = dot(t1,vv);

t2 = GetaT_vec(invP,j,v);
t2 = dot(t2,w);

%% Test the adjoint problem (PASSED)

clear
invP = invParams;

%Mesh
nc = 6;
invP.mesh = meshDM(10*ones(nc,1),10*ones(nc,1),10*ones(nc,1));
invP.activeCells = invP.mesh.CC(:,3)<sum(invP.mesh.hz)/2;

%Tx/Rx locations
invP.loc = [sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2];

%Time stepping
invP.disT = [3 1e-6];
   
%Primary Fields   
invP.h0 = calcH0_VMD(invP);

%Interpolation matrix
invP.Q = getInterpmat(invP);

%Sigma and tau models
invP.sigmaModel = 1e-8*ones(invP.mesh.nc,1);
invP.sigmaModel(invP.activeCells) = 1e-3;

invP.tauModel = zeros(invP.mesh.nc,1);
invP.tauModel(invP.activeCells) = 0.1;
 
% Make a model

k = 1/invP.disT(1,2);

m = abs(rand(invP.mesh.nc,1))/2 + 0.1;

T = sdiag(invP.mesh.Af*(1 + k*invP.tauModel.*(1 - m)));
Tt = sdiag(invP.mesh.Af*(k*invP.tauModel.*(1 - m)));

M = k*invP.mu*speye(invP.mesh.ne);
S = sdiag(invP.mesh.Af*(invP.sigmaModel.*(1 + k*invP.tauModel)));
St = sdiag(invP.mesh.Af*(k*invP.tauModel.*invP.sigmaModel));

% Build Ah

A = [M invP.mesh.CURL' zeros(invP.mesh.ne,invP.mesh.nf);
    invP.mesh.CURL zeros(invP.mesh.nf,invP.mesh.nf) -speye(invP.mesh.nf);
    zeros(invP.mesh.nf,invP.mesh.ne) S -T];

B = [-M zeros(size(invP.mesh.CURL')) zeros(invP.mesh.ne,invP.mesh.nf);
    zeros(size(invP.mesh.CURL)) zeros(invP.mesh.nf) zeros(invP.mesh.nf);
    zeros(invP.mesh.nf,invP.mesh.ne) -St Tt];
Ah = [A zeros(size(A)) zeros(size(A)); B A zeros(size(A)); zeros(size(A)) B A];

qh = [];
for ii = 1:invP.nt
    qh{ii} = rand(invP.mesh.ne,invP.ntx);
end

qe = [];
for ii = 1:invP.nt
    qe{ii} = zeros(invP.mesh.nf,invP.ntx);
end

qj = [];
for ii = 1:invP.nt
    qj{ii} = zeros(invP.mesh.nf,invP.ntx);
end

q = [];
for ii = 1:invP.nt
    q = [q; qh{ii}; qe{ii}; qj{ii}];
end

u1 = (Ah')\q;


uj1 = [];
for ii = 1:invP.ntx
    tmp = u1(:,ii);
    tmp = reshape(tmp,invP.mesh.nf*2 + invP.mesh.ne,invP.nt);
    tmp = tmp(invP.mesh.ne+invP.mesh.nf+1:end,:);
    uj1(:,ii) = tmp(:);
end


uj2 = calcAhTInv(invP,m,qh);
uj2 = cell2mat(uj2);



figure(1);
plot(uj1(:),'o');
hold on;
plot(uj2(:),'rx');
hold off;

%% Adjoint test of Qvec and QTvec (PASSED)


clear
invP = invParams;

%Mesh
nc = 12;
invP.mesh = meshDM(10*ones(nc,1),10*ones(nc,1),10*ones(nc,1));
invP.activeCells = invP.mesh.CC(:,3)<sum(invP.mesh.hz)/2;

%Tx/Rx locations
invP.loc = [sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2;
    sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2];

%Time stepping
invP.disT = [3 1e-6];
   
%Primary Fields   
invP.h0 = calcH0_VMD(invP);

%Interpolation matrix
invP.Q = getInterpmat(invP);

%Random fields
h = [];
for ii = 1:invP.nt
    h{ii,1} = rand(invP.mesh.ne,invP.ntx);
end
hv = cell2mat(h);
hv = hv(:);

%Random data
d = rand(invP.nt,invP.ntx);

Qv = projHs(invP,h);
wtQv = dot(d(:),Qv(:));

Qtw = projTHs(invP,d);
Qtw = cell2mat(Qtw);
Qtw = Qtw(:);
vtQtw = dot(hv,Qtw);

wtQv - vtQtw


%% Adjoint test of Jvec and JTvec (PASSED)

clear
invP = invParams;

%Mesh
nc = 12;
invP.mesh = meshDM(10*ones(nc,1),10*ones(nc,1),10*ones(nc,1));
invP.activeCells = invP.mesh.CC(:,3)<sum(invP.mesh.hz)/2;

%Tx/Rx locations
invP.loc = [sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2+10 sum(invP.mesh.hx)/2
            sum(invP.mesh.hx)/2 sum(invP.mesh.hx)/2-10 sum(invP.mesh.hx)/2];
        
%Time stepping
invP.disT = [10 1e-6];
   
%Primary Fields   
invP.h0 = calcH0_VMD(invP);

%Interpolation matrix
invP.Q = getInterpmat(invP);

%Sigma and tau models
invP.sigmaModel = 1e-8*ones(invP.mesh.nc,1);
invP.sigmaModel(invP.activeCells) = 1e-3;

invP.tauModel = zeros(invP.mesh.nc,1);
invP.tauModel(invP.activeCells) = 0.1;

etaModel = rand(invP.mesh.nc,1);

%Random model vector
m = rand(invP.mesh.nc,1);

%Random data
d = rand(invP.nt,invP.ntx);

%Random current
j = [];
for ii = 1:invP.nt
    j{ii} = rand(invP.mesh.nf,invP.ntx);
end

Jm = Jvec_TDIP(invP,etaModel,j,m);
dtJm = dot(d(:),Jm(:));

Jtd = JTvec_TDIP(invP,etaModel,j,d);
mtJtd = dot(m(:),Jtd(:));

dtJm - mtJtd

